/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file template.cxx
  \brief class implemenation for template

  \author Jack Elston
  $Date: 2005/02/12 00:20:45 $
*/

#include <iostream>
#include <stdio.h>

#include "track.h"
#include "apSim.h"
#include "color.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---Global Variables---->*/
extern bool verbose;
/*<-End Global Variables-->*/

Track::Track(String name) : ThreadedPipe(name)
{
	last_tracking_loop.stamp();
	last_client_update.stamp();

	cout << GREEN << "---------->>>>>      " YELLOW "TRACK" GREEN "      <<<<<----------" << ATTROFF << endl;
	fc_mode = STANDBY;
	send_goto_wp = 0;
}

void Track::update() 
{
	bool got_info = false;
	int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;

	FD_ZERO(&readFDs);	             		// zero read fd set
	FD_ZERO(&writeFDs);	             		// zero write fd set
	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
	//FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
	//FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

	//fifo_FDs[INBOARD_RX] > fifo_FDs[OUTBOARD_RX] ? maxFD = fifo_FDs[INBOARD_RX] : maxFD = fifo_FDs[OUTBOARD_RX];
	maxFD = fifo_FDs[INBOARD_RX];
	if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
	//if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	Client * me = shmPtr->clients.getClient(0);

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("Track::update() select");
				execRate = -1;
			} else
				cout << getName() << "::update() - select interrupted" << endl;
		}
	}
	else {
#if 0
		if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
		{
			int n = 0;
			n=readFifo(OUTBOARD_RX);
			if(n>0) {
				uint16_t length = CMD_SIZE(fifo_msg.data);
				if( length != fifo_msg.size) {
					cout << getName() << "::update() - error, length of command != length of buffer" << endl
						<< "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

					if( length < 1000 && length <= fifo_msg.size )
						if( C_calcCheckSum((uint8_t *)buf) == 0 ) 
							cout << "\tCMD: is valid" << endl;
						else
							cout << "\tCMD: is NOT valid" << endl;

					return;
				}

				Command cmd(fifo_msg.data);

				if( !cmd.isValid() )
					cout << getName() << "::update() - error, invalid command" << endl << cmd ;
				else {
					switch(cmd.cmd() ) {
						case CMD_RETURN: {
								if(tracked_vehicles.getNumClients() != 0) {
									Client * me = shmPtr->clients.getClient(0);

									CMD_CMD(fifo_msg.data)  = CMD_RETURN;
									CMD_NUM(fifo_msg.data)  = 0;
									CMD_CSUM(fifo_msg.data) = 0;
									CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

									fifo_msg.size = CMD_SIZE(fifo_msg.data);
									fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr;
									fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
									writeFifo(OUTBOARD_TX);
								}

								tracked_vehicles.clear(); 
								me->telemetry.navigation.OBmode = OB_OFF; 
								me->telemetry.navigation.OBtype = OB_NONE;
							} break;
						default:
								writeFifo(INBOARD_TX);
							break;
					}
				}
			}
		}
#endif

		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
			int n = 0;
			n=readFifo(INBOARD_RX);
			if(n>0) {
				uint16_t length = CMD_SIZE(fifo_msg.data);
				if( length != fifo_msg.size) {
					cout << getName() << "::update() - error, length of command != length of buffer" << endl
						<< "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

					if( length < 1000 && length <= fifo_msg.size ){
						if( C_calcCheckSum((uint8_t *)buf) == 0 ) 
							cout << "\tCMD: is valid" << endl;
						else
							cout << "\tCMD: is NOT valid" << endl;
					}

					return;
				}

				Command cmd(fifo_msg.data);

				if( !cmd.isValid() )
					cout << getName() << "::update() - error, invalid command" << endl << cmd ;
				else {
					Client * a_client;
					a_client = shmPtr->clients.getClient(fifo_msg.src);
					if(a_client != NULL) {

						uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);

						switch(cmd.cmd() ) {
							case CMD_TRACK: {
									int num_vehicles = cmd.size()/sizeof(VehicleInfo_t);
									int sub_sum = 0;

									// save the src addr
									in_addr_t src;
									src = fifo_msg.src.s_addr;

									if(num_vehicles == 0) {
										CMD_CMD(fifo_msg.data)  = CMD_RETURN;
										CMD_NUM(fifo_msg.data)  = 0;
										CMD_CSUM(fifo_msg.data) = 0;
										CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

										fifo_msg.size = CMD_SIZE(fifo_msg.data);
										fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr;
										fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
										//writeFifo(OUTBOARD_TX);
										writeFifo(INBOARD_TX);
										cout << "---------->>>>>   STOPPING TRACK   <<<<<----------" << endl;
									}

									if( !(me->telemetry.navigation.OBtype == OB_NONE || me->telemetry.navigation.OBtype == OB_TRACK) )  {
										NACK(INBOARD_TX);
										break;
									}

									me->telemetry.navigation.OBmode = OB_OFF; 
									me->telemetry.navigation.OBtype = OB_TRACK; 
									tracked_vehicles.clear();

									if( num_vehicles > 0 ) {
										me->telemetry.navigation.OBmode = OB_INIT; 
										for(int j=0;j<num_vehicles;j++){
											if((a_client = shmPtr->clients.getClient(((VehicleInfo_t*)dataPtr)[j].id)) != NULL) {
												if(tracked_vehicles.add(a_client)) {
													cout << "requresting TELEM stream" << endl;
													subscribe(CMD_GPS_TELEM, a_client, true, INBOARD_TX);
													sub_sum++;
												}
											}
										}
										if( sub_sum > 0 ) {
											// setup command again to ack the sender of the track command
											fifo_msg.src.s_addr = src;
											CMD_CMD(fifo_msg.data) = CMD_TRACK;
											ACK(INBOARD_TX);

											cout << "---------->>>>>       TRACK        <<<<<----------" << endl;
											fc_mode = TRACK;

											cout << "TRACKING " << tracked_vehicles.getNumClients() << " VEHICLES" << endl;
											// Send goto wp command
											send_goto_wp = 1;



										} else {
											NACK(INBOARD_TX);
											tracked_vehicles.clear();
											me->telemetry.navigation.OBmode = OB_ERROR; 
										}

									} else
										ACK(INBOARD_TX);
								} break;
							case CMD_ACK: {
									;//cout << "CMD_ACK from " << a_client->name << endl;
									break;
								} 
							case CMD_FAIL: {
									 //cout << "CMD_FAIL from " << a_client->name << endl;
									 break;
								} 
							case CMD_RETURN: {
									if( fc_mode != STANDBY ) {
											printf(" stopping tracking controller\n");
											tracked_vehicles.clear(); 
											me->telemetry.navigation.OBmode = OB_OFF; 
											me->telemetry.navigation.OBtype = OB_NONE;
									}
									printf("\n");
									break;
							} 
							case CMD_GPS_TELEM: {
									GPSTelem_t * telemetry  = (GPSTelem_t*)dataPtr;
									Client * tracked_client = tracked_vehicles.getClient(a_client->name);
									if(tracked_client != NULL) {
										tracked_client->telemetry.gps.pos.lat=telemetry->pos.lat;
										tracked_client->telemetry.gps.pos.lon=telemetry->pos.lon;
										tracked_client->telemetry.gps.track=telemetry->track;
										tracked_client->telemetry.gps.sog=telemetry->sog;
										if( telemetry->pos.lat != 0 && telemetry->pos.lon != 0) {
											got_info = true;
											last_client_update.stamp();
										}
									}
								} break;
							case CMD_ORBIT_VALUES: {
									CopyBufToStruct(dataPtr, orbit);

									// reply back that we got message
									ACK(INBOARD_TX);
								} break;
							case CMD_FP: {

									//writeFifo(OUTBOARD_TX);

									if( a_client != me && me->telemetry.navigation.OBtype == OB_TRACK) {
										if(tracked_vehicles.getNumClients() != 0) {
											Client * me = shmPtr->clients.getClient(0);

											CMD_CMD(fifo_msg.data)  = CMD_RETURN;
											CMD_NUM(fifo_msg.data)  = 0;
											CMD_CSUM(fifo_msg.data) = 0;
											CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

											fifo_msg.size = CMD_SIZE(fifo_msg.data);
											fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr;
											fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
											//writeFifo(OUTBOARD_TX);
											writeFifo(INBOARD_TX);
										}

										if( me->telemetry.navigation.OBtype == OB_TRACK ) {
											me->telemetry.navigation.OBmode = OB_OFF; 
											me->telemetry.navigation.OBtype = OB_NONE; 
										}
										tracked_vehicles.clear();
									}
								} break;
							default:
								//writeFifo(OUTBOARD_TX);
								break;
						}
					}
				}
			}
		}
	}
/* old method
	currentStamp.stamp();
	if((currentStamp - last_tracking_loop ) > (TRACKING_LOOP_PERIOD * SEC2MICRO) ) {
		for(int i=0; i< tracked_vehicles.getNumClients(); i ++)
			if(((tracked_vehicles.getClient(i))->telemetry.gps.pos.lat) == 0.0) {
				cout << "requesting TELEM stream" << endl;
				subscribe(CMD_GPS_TELEM, tracked_vehicles.getClient(i), true, INBOARD_TX);
				got_info = false;
			}
		if((tracked_vehicles.getNumClients() > 0) && got_info) { trackTargets(); } 

		last_tracking_loop.stamp();
	}
*/

	currentStamp.stamp();
	if((currentStamp - last_client_update ) > (TRACKING_CLIENT_TIMEOUT * SEC2MICRO) ) {
		got_info = false;
		for(int i=0; i< tracked_vehicles.getNumClients(); i ++) {
			cout << "requesting TELEM stream" << endl;
			subscribe(CMD_GPS_TELEM, tracked_vehicles.getClient(i), true, INBOARD_TX);
		}
		last_client_update.stamp();
	}

	if((currentStamp - last_tracking_loop ) > (TRACKING_LOOP_PERIOD * SEC2MICRO) ) {
		if((tracked_vehicles.getNumClients() > 0) && got_info) { 
			trackTargets(); 
			last_tracking_loop.stamp();
			
			// Send goto wp command
			if(send_goto_wp){
				GotoWP_t wp;
				wp.ind = orbit.wp;

				CMD_CMD(fifo_msg.data)  = CMD_GOTO_WP;
				CMD_NUM(fifo_msg.data)  = sizeof(GotoWP_t);
				CopyStructToBuf(wp, CMD_DATAPTR(fifo_msg.data) );
				CMD_CSUM(fifo_msg.data) = 0;
				CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

				fifo_msg.size = CMD_SIZE(fifo_msg.data);
				fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr;
				fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
				writeFifo(INBOARD_TX);
			 
				send_goto_wp = 0;
			}
		} 
	}
}

void Track::trackTargets() {
	GPSLatLon_t orbit_point;
	orbit_point.lat=0.0;
	orbit_point.lon=0.0;

	Client * me = shmPtr->clients.getClient(0);

	printf("TRACKING: \n");
	for(int j=0;j<tracked_vehicles.getNumClients();j++) {
		orbit_point.lat += tracked_vehicles.getClient(j)->telemetry.gps.pos.lat+M_TO_LAT(orbit.offset[0]);
		orbit_point.lon += tracked_vehicles.getClient(j)->telemetry.gps.pos.lon+M_TO_LON(orbit.offset[1],orbit_point.lat);

		printf("\t%s (%f, %f)\n", tracked_vehicles.getClient(j)->name.c_str(),  
				tracked_vehicles.getClient(j)->telemetry.gps.pos.lat, 
				tracked_vehicles.getClient(j)->telemetry.gps.pos.lon);
	}
	printf("\n");

	// orbit point is average of all tracked node locations
	orbit_point.lat /= tracked_vehicles.getNumClients();
	orbit_point.lon /= tracked_vehicles.getNumClients();

#ifdef USE_AUTOPILOT_ORBIT
	/*----------[ using the autopilot to orbit ]----------*/
	me->telemetry.navigation.OBmode = OB_ORBIT; 

	Waypt_t * waypoint = (Waypt_t*)CMD_DATAPTR(fifo_msg.data);
	waypoint[0].number   = orbit.wp;
	waypoint[0].pos.lat  = orbit_point.lat;
	waypoint[0].pos.lon  = orbit_point.lon;
	waypoint[0].pos.alt  = orbit.alt;
	waypoint[0].orbitRad = (int16_t)(orbit.dir * orbit.rad); 
	waypoint[0].next     = orbit.wp + 1;

	waypoint[1].number   = orbit.wp + 1;
	waypoint[1].pos.lat  = me->telemetry.gps.pos.lat;
	waypoint[1].pos.lon  = me->telemetry.gps.pos.lon;
	waypoint[1].pos.alt  = orbit.alt;
	waypoint[1].orbitRad = 0;
	waypoint[1].next     = orbit.wp;

	CMD_CMD(fifo_msg.data)  = CMD_FP;
	CMD_NUM(fifo_msg.data)  = 2*sizeof(Waypt_t);
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);
#else
	/*----------[ LGVF controller to orbit ]----------*/
	me->telemetry.navigation.OBmode = OB_TURNRATE; 

	float psides;
	float psidotdes;

	vectorField(me->telemetry.gps.pos, me->telemetry.gps.track, 
	       orbit_point, orbit.rad, me->telemetry.airdata.tas, &psides, &psidotdes);

	APRates_t * ap_rates = (APRates_t*)CMD_DATAPTR(fifo_msg.data);

	ap_rates->heading  = psides;
	ap_rates->velocity = 0;

	CMD_CMD(fifo_msg.data)  = CMD_AP_RATES;
	CMD_NUM(fifo_msg.data)  = sizeof(APRates_t);
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);
#endif


	// send the command to myself, for the thread that handles piccolo interface
	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
	fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr;
	writeFifo(INBOARD_TX);

	//fifo_msg.size = CMD_SIZE(fifo_msg.data);
	//writeFifo(OUTBOARD_TX);
}
